#include "gimbal_task.h"
#include "CAN_receive.h"

#include "sen_ug_ctr/pid.h"
#include "sen_ug_ctr/user_lib.h"
#include "sen_ug_ctr/struct_typedef.h"

#define PITCH_TEST_RAD_MIN (-0.4f)
#define PITCH_TEST_RAD_MAX (0.48f)

/**********************PID控制参数********************/

#define PITCH_AGL_KP 15.0f
#define PITCH_AGL_KI 0.0f
#define PITCH_AGL_KD 0.0f
#define PITCH_AGL_SPD_MAX_OUT (30.0f)
#define PITCH_AGL_SPD_MAX_IOUT (0.7f)

#define PITCH_SPD_KP 1200.0f
#define PITCH_SPD_KI 0.5f
#define PITCH_SPD_KD 3.0f
#define PITCH_VOLT_MAX_OUT 6000.0f
#define PITCH_VOLT_MAX_IOUT 40.0f

#define YAW_AGL_KP 25.0f
#define YAW_AGL_KI 0.0f
#define YAW_AGL_KD 0.0f
#define YAW_AGL_SPD_MAX_OUT (80.0f)
#define YAW_AGL_SPD_MAX_IOUT (0.7f)

#define YAW_SPD_KP 110000.0f
#define YAW_SPD_KI 0000.0f
#define YAW_SPD_KD 000.0f
#define YAW_VOLT_MAX_OUT 30000.0f
#define YAW_VOLT_MAX_IOUT 5000.0f

#define PID_DEATH_AREA 0.005 //防止I不断累积

// 一阶滤波器的参数,专门给。这两个值越大，理论上越丝滑
#define PITCH_SPD_FILTER_NUM 15.0f
#define YAW_SPD_FILTER_NUM 20.0f

#define rc_deadband_limit(input, output, dealine)        \
    {                                                    \
        if ((input) > (dealine) || (input) < -(dealine)) \
        {                                                \
            (output) = (input);                          \
        }                                                \
        else                                             \
        {                                                \
            (output) = 0;                                \
        }                                                \
    }

#define range_limit_inside(toBeLimited, range) \
    {                                          \
        if ((toBeLimited) > (range))           \
        {                                      \
            (toBeLimited) = (range);           \
        }                                      \
        else if ((toBeLimited) < -(range))     \
        {                                      \
            (toBeLimited) = -(range);          \
        }                                      \
    }

struct gimbalMotorCtrl_t
{
    // rads
    const fp32 *anglePoint;   // 从姿态解算任务得到的角度位置
    const fp32 *gyroPointer;  // 陀螺仪角速度指针
    fp32 nowAbsoluteAngle;    // 当前姿态角
    fp32 radSpeed;            // 角速度 rad/s
    fp32 wantedAbsoluteAngle; // 目标姿态角

    // PIDs
    pid_type_def spdPid; // 用于控制速度的PID环。输入角速度 rad/s, 输出GM6020电压 int16
    pid_type_def aglPid; // 用于控制角度的PID环。输入角度 rad,输出角速度rad/s

    // 控制量
    int16_t giveVolt; // GM6020发送电压
    uint8_t zeroVoltMark;

    // 一阶滤波器，过滤角速度
    first_order_filter_type_t spd_filter;
};

static struct gimbalMotorCtrl_t gimbalPitchCtrl, gimbalYawCtrl;
static struct gimbalMotorCtrl_t *gimbalCtrl[2] = {&gimbalPitchCtrl, &gimbalYawCtrl};

static fp32 yawSpdPID[3] = {YAW_SPD_KP, YAW_SPD_KI, YAW_SPD_KD};
static fp32 pitchSpdPID[3] = {PITCH_SPD_KP, PITCH_SPD_KI, PITCH_SPD_KD};
static fp32 yawAglPID[3] = {YAW_AGL_KP, YAW_AGL_KI, YAW_AGL_KD};
static fp32 pitchAglPID[3] = {PITCH_AGL_KP, PITCH_AGL_KI, PITCH_AGL_KD};

static void init_gimbal_ctrls(void)
{

    // 初始化PID参数
    // 两个双环PID不共用参数
    PID_init(&(gimbalPitchCtrl.spdPid), PID_POSITION, pitchSpdPID, PITCH_VOLT_MAX_OUT, PITCH_VOLT_MAX_IOUT);
    PID_init(&(gimbalYawCtrl.spdPid), PID_POSITION, yawSpdPID, YAW_VOLT_MAX_OUT, YAW_VOLT_MAX_IOUT);

    PID_init(&(gimbalPitchCtrl.aglPid), PID_POSITION, pitchAglPID, PITCH_AGL_SPD_MAX_OUT, PITCH_AGL_SPD_MAX_IOUT);
    PID_init(&(gimbalYawCtrl.aglPid), PID_POSITION, yawAglPID, YAW_AGL_SPD_MAX_OUT, YAW_AGL_SPD_MAX_IOUT);

    // 滤波初始化
    const static fp32 gimbal_pitch_order_filter[1] = {PITCH_SPD_FILTER_NUM};
    const static fp32 gimbal_yaw_order_filter[1] = {YAW_SPD_FILTER_NUM};

    first_order_filter_init(&(gimbalPitchCtrl.spd_filter), GIMBAL_TASK_CTRL_TIME, gimbal_pitch_order_filter);
    first_order_filter_init(&(gimbalYawCtrl.spd_filter), GIMBAL_TASK_CTRL_TIME, gimbal_yaw_order_filter);

    // 初始化当前角度和目标角度、pid参数、里程碑栈法数据
    for (i = 0; i < 2; i++)
    {
        (gimbalCtrl[i])->zeroVoltMark = 0;
        (gimbalCtrl[i])->nowAbsoluteAngle = *((gimbalCtrl[i])->anglePoint);
        gimbalCtrl[i]->wantedAbsoluteAngle = (gimbalCtrl[i])->nowAbsoluteAngle;
    }
    PID_clear(&gimbalYawCtrl.aglPid);
    PID_clear(&gimbalYawCtrl.spdPid);
    PID_clear(&gimbalPitchCtrl.aglPid);
    PID_clear(&gimbalPitchCtrl.spdPid);
    gimbalYawCtrl.radSpeed = 0;
    gimbalPitchCtrl.radSpeed = 0;
}

fp32 radFormat(fp32 rawAngle)
{
    while (rawAngle > PI)
        rawAngle -= (2 * PI);
    while (rawAngle < (-PI))
        rawAngle += (2 * PI);
    return rawAngle;
}

fp32 gimbal_PID_calc(pid_type_def *pid, fp32 ref, fp32 set)
{
    if (pid == NULL)
    {
        return 0.0f;
    }

    pid->error[2] = pid->error[1];
    pid->error[1] = pid->error[0];
    pid->set = set;
    pid->fdb = ref;
    pid->error[0] = radFormat(set - ref);
    if (pid->mode == PID_POSITION)
    {
        pid->Pout = pid->Kp * pid->error[0];
        pid->Iout += pid->Ki * pid->error[0];
        pid->Dbuf[2] = pid->Dbuf[1];
        pid->Dbuf[1] = pid->Dbuf[0];
        pid->Dbuf[0] = (pid->error[0] - pid->error[1]);
        pid->Dout = pid->Kd * pid->Dbuf[0];
        range_limit_inside(pid->Iout, pid->max_iout);
        pid->out = pid->Pout + pid->Iout + pid->Dout;
        range_limit_inside(pid->out, pid->max_out);
    }
    else if (pid->mode == PID_DELTA)
    {
        pid->Pout = pid->Kp * (pid->error[0] - pid->error[1]);
        pid->Iout = pid->Ki * pid->error[0];
        pid->Dbuf[2] = pid->Dbuf[1];
        pid->Dbuf[1] = pid->Dbuf[0];
        pid->Dbuf[0] = (pid->error[0] - 2.0f * pid->error[1] + pid->error[2]);
        pid->Dout = pid->Kd * pid->Dbuf[0];
        pid->out += pid->Pout + pid->Iout + pid->Dout;
        range_limit_inside(pid->out, pid->max_out);
    }
    return pid->out;
}

void refresh_angle_states(void)
{
    // 当前姿态角和角速度
    gimbalYawCtrl.nowAbsoluteAngle = *(gimbalYawCtrl.anglePoint);
    gimbalYawCtrl.radSpeed = *(gimbalYawCtrl.gyroPointer);
    #ifdef INFANTRY
    gimbalPitchCtrl.nowAbsoluteAngle = *(gimbalPitchCtrl.anglePoint);
    gimbalPitchCtrl.radSpeed = *(gimbalPitchCtrl.gyroPointer);
    #else
    gimbalPitchCtrl.nowAbsoluteAngle =-*(gimbalPitchCtrl.anglePoint);
    gimbalPitchCtrl.radSpeed =-*(gimbalPitchCtrl.gyroPointer);
    #endif
}
// #ifdef STM_STUDIO_PITCH_CHANNEL
// int16_t stm_pitch_channel;
// #endif


void get_control_angles(void)
{
    gimbalYawCtrl.wantedAbsoluteAngle = ... ;
    gimbalPitchCtrl.wantedAbsoluteAngle = ... ;
}

void limit_angles()
{
    struct gimbalMotorCtrl_t *c;
    int i;
    for (i = 0; i < 2; i++)
    {
        c = gimbalCtrl[i];
        if (radFormat(c->wantedAbsoluteAngle - c->nowAbsoluteAngle) > (PI / 2))
            c->wantedAbsoluteAngle = c->nowAbsoluteAngle + (PI / 2);
        if (radFormat(c->wantedAbsoluteAngle - c->nowAbsoluteAngle) < (-PI / 2))
            c->wantedAbsoluteAngle = c->nowAbsoluteAngle - (PI / 2);
    }
    if (radFormat(gimbalPitchCtrl.wantedAbsoluteAngle) > PITCH_TEST_RAD_MAX)
        gimbalPitchCtrl.wantedAbsoluteAngle = PITCH_TEST_RAD_MAX;
    if (radFormat(gimbalPitchCtrl.wantedAbsoluteAngle) < PITCH_TEST_RAD_MIN)
        gimbalPitchCtrl.wantedAbsoluteAngle = PITCH_TEST_RAD_MIN;
}

// #ifdef STM_STUDIO_PITCH_PID
// #define LimitMax(input, max)   \
//     {                          \
//         if (input > max)       \
//         {                      \
//             input = max;       \
//         }                      \
//         else if (input < -max) \
//         {                      \
//             input = -max;      \
//         }                      \
//     }

// fp32 pidPout,pidIout,pidDout;
// fp32 PID_calc_STM_STUDIO_PITCH_PID(pid_type_def *pid, fp32 ref, fp32 set,uint8_t i)
// {
//     if (pid == NULL)
//     {
//         return 0.0f;
//     }

//     pid->error[2] = pid->error[1];
//     pid->error[1] = pid->error[0];
//     pid->set = set;
//     pid->fdb = ref;
//     pid->error[0] = set - ref;
//     if (pid->mode == PID_POSITION)
//     {
//         pid->Pout = pid->Kp * pid->error[0];
//         pid->Iout += pid->Ki * pid->error[0];
//         pid->Dbuf[2] = pid->Dbuf[1];
//         pid->Dbuf[1] = pid->Dbuf[0];
//         pid->Dbuf[0] = (pid->error[0] - pid->error[1]);
//         pid->Dout = pid->Kd * pid->Dbuf[0];
//         LimitMax(pid->Iout, pid->max_iout);
//         pid->out = pid->Pout + pid->Iout + pid->Dout;
//         LimitMax(pid->out, pid->max_out);
//     }
//     else if (pid->mode == PID_DELTA)
//     {
//         pid->Pout = pid->Kp * (pid->error[0] - pid->error[1]);
//         pid->Iout = pid->Ki * pid->error[0];
//         pid->Dbuf[2] = pid->Dbuf[1];
//         pid->Dbuf[1] = pid->Dbuf[0];
//         pid->Dbuf[0] = (pid->error[0] - 2.0f * pid->error[1] + pid->error[2]);
//         pid->Dout = pid->Kd * pid->Dbuf[0];
//         pid->out += pid->Pout + pid->Iout + pid->Dout;
//         LimitMax(pid->out, pid->max_out);
//     }
//     if(i==0){
//         pidPout=pid->Pout;
//         pidIout=pid->Iout;
//         pidDout=pid->Dout;
//     }
//     return pid->out;
// }
// #endif

void calc_PID(void)
{
    uint8_t i;
    struct gimbalMotorCtrl_t *c;
    // #ifndef SENTRY
    for (i = 0; i < 2; i++)
    {
        c = gimbalCtrl[i];
        gimbal_PID_calc(&(c->aglPid), c->nowAbsoluteAngle, c->wantedAbsoluteAngle); // 关乎旋转方向的PID控制器
        PID_calc(&(c->spdPid), c->radSpeed, (c->aglPid).out);                       // 普通的速度控制环
        c->giveVolt = c->spdPid.out;                                                // 给电压
        if (c->zeroVoltMark == 1)
        {
            c->giveVolt = 0;
        }
    }
    // #else
    // c = gimbalCtrl[0];
    // gimbal_PID_calc(&(c->aglPid), c->nowAbsoluteAngle, c->wantedAbsoluteAngle); // 关乎旋转方向的PID控制器
    // // #ifndef STM_STUDIO_PITCH_PID
    // PID_calc(&(c->spdPid), ((c->radSpeed-(c->aglPid).out)>PID_DEATH_AREA||(c->radSpeed-(c->aglPid).out)<-PID_DEATH_AREA?c->radSpeed:(c->aglPid).out), (c->aglPid).out);                       // 普通的速度控制环
    // // #else
    // // PID_calc_STM_STUDIO_PITCH_PID(&(c->spdPid), ((c->radSpeed-(c->aglPid).out)>PID_DEATH_AREA||(c->radSpeed-(c->aglPid).out)<-PID_DEATH_AREA?c->radSpeed:(c->aglPid).out), (c->aglPid).out,i);
    // // #endif
    // c->giveVolt = c->spdPid.out;                                                // 给电压
    // if (c->zeroVoltMark == 1)
    // {
    //     c->giveVolt = 0;
    // }
    // c = gimbalCtrl[1];
    // gimbal_PID_calc(&(c->aglPid), c->nowAbsoluteAngle, c->wantedAbsoluteAngle); // 关乎旋转方向的PID控制器
    // PID_calc(&(c->spdPid), c->radSpeed, (c->aglPid).out);                         // 普通的速度控制环
    // c->giveVolt = c->spdPid.out;                                                // 给电压
    // if (c->zeroVoltMark == 1)
    // {
    //     c->giveVolt = 0;
    // }
    // #endif
}

#ifdef GIMBAL_WITH_IMU

void gimbal_with_IMU_task(void const *pvParameters)
{
    init_gimbal_ctrls();

    while (1)
    {
        refresh_angle_states(); // 从传感器获得当前角度数据 
        get_control_angles();   // 从遥控器获得目标角度     
        limit_angles();
        calc_PID(); // 总是控制角度，双环，但测试时需要
        // 发送
        osDelay(GIMBAL_TASK_CTRL_TIME);
    }
}
#endif

